// motor.c
#include "motor.h"

/**
 * @brief 初始化电机对象
 */
void motor_init(Motor *m, GPIO_PinName pin1, GPIO_PinName pin2, GPIO_PinName pwm_pin)
{
    m->pin1 = pin1;
    m->pin2 = pin2;
    m->pwm_pin = pwm_pin;
    m->direction = MOTOR_STOP;
    m->throttle = 0;

    // 配置引脚模式
    pinMode(pin1, OUTPUT);
    pinMode(pin2, OUTPUT);
    pinMode(pwm_pin, OUTPUT_AF_PP);  // 复用推挽（PWM）
}

/**
 * @brief 设置电机方向和油门
 * @param direction: 0=停, 1=正, 2=反
 * @param speed: 0~1000（对应 0.0% ~ 100.0% 占空比）
 */
void motor_set(Motor *m, int8_t direction, uint16_t thr)
{
    m->direction = direction;
    m->throttle = (thr > 1000) ? 1000 : thr;

    // 设置方向
    switch (direction) {
        case MOTOR_FWD:
            digitalWrite(m->pin1, 1);
            digitalWrite(m->pin2, 0);
            break;
        case MOTOR_REV:
            digitalWrite(m->pin1, 0);
            digitalWrite(m->pin2, 1);
            break;
        default:
        case MOTOR_STOP:
            digitalWrite(m->pin1, 0);
            digitalWrite(m->pin2, 0);
            break;
    }

    // 设置 PWM 速度
    analogWrite(m->pwm_pin, m->throttle);
}

/**
 * @brief 停止电机
 */
void motor_stop(Motor *m)
{
    motor_set(m, MOTOR_STOP, 0);
}
